/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: PositionRanging_Logger.cpp,v 1.2 2009-05-13 07:18:23 amaula Exp $
 *
 * \file
 * \brief Dummy implementation for providing Position and Ranging interfaces.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * This implementation can act as an example/base for Jari 
 * for implementing localization modules. :)
 *
 ******************************************************************************
 * Module Info:
 *
 * Provides Interfaces:
 * - Position
 * - Ranging
 *
 * Needs Interfaces:
 * - Position
 * - Ranging
 *
 * General information:
 * - This is a localization module, producing new position from an existing
 * position source combined with ranging information.
 *  
 * \see MaCI::Position
 * \see MaCI::Ranging
 ******************************************************************************
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "PositionClient.hpp"
#include "RangingClient.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
static volatile bool run = true;
static ownThreadHandle signalHandlerThread;
static gimi::GIMI g;
std::string logfilename="position_ranging_log.txt";
static bool disablePosition=false; ///< set true if no position server available
//*****************************************************************************
using namespace MaCI;
//*****************************************************************************

static int SignalHandlerThread(void *)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(1,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      dPrint(1,"Terminating...");
      run = false;
      g.stop();
      dPrint(1,"Waiting 3 seconds for cleanup...");
      ownSleep_ms(2000);
      dPrint(1,"Exit.");
      ownSleep_ms(1000);
      break;
      
    default:
      dPrint(1,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************

static void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tPositionRanging_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-g name]\t\t\tUse 'name' as this nodes MaCI-Group\n");

  fprintf(stderr,"\nApplication specific:\n");
  fprintf(stderr,"  [-P name]\t\tUse 'name' as Position source Instance \n");
  fprintf(stderr,"  [-R name]\t\tUse 'name' as Ranging source Instance\n");
	fprintf(stderr,"  [-I name]\t\tSet 'name' as Module Instance name\n");
	fprintf(stderr,"  [-f name]\t\tUse 'Name' Logfile\n");
	fprintf(stderr,"  [-d]\t\t Disable position interface (don,t connect to a server)!\n");
  
	fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

static int ExecuteModule(Position::CPositionClient &positionClient,
                         Ranging::CRangingClient &rangingClient) 
{
  dPrint(1,"ExecuteModule called.");

  // Variables for accessing position
  Position::CPositionData pd;
  Position::CPositionData pd_timed;

  // Variables for accessing ranging
  Ranging::CRangingData rd;
  int rangingSeq = -1;
  
  // Common variables
  bool r,rr,pr;
  gim::time measure_diff;
  gim::time total_drift_immediate;
  gim::time total_drift_timed;

	FILE *f = fopen(logfilename.c_str(),"wt");
	const Ranging::TDistance *r_meas;
	
  // Run in loop doing stuff. 
  while(run) {
    // Get Ranging event.
    // Note: There is an overloaded version of this function for easier access to measures.
    // See PositionClient.hpp for details (or generated documentation)
    rr = rangingClient.GetDistanceArray(rd, &rangingSeq, 1000);
    if (!rr) {
      dPrint(1,"Failed to get DistanceArray?");
      continue;
    }

    // This sleep is to verify that we already have a position
    // matching the Distance.  It is not compulsory, but guarantees
    // that the Position matches the Ranging information.
    ownSleep_ms(20);    
    
		if(!disablePosition){
				// Get best time-matching position for the received DistanceArray
				const Common::TTimestamp *ts = rd.GetTimestamp(); // Timestamp from distancearray
				if (ts) {
				// Got timestamp.
				pr = positionClient.GetPositionEvent(pd_timed, ts->GetGimTime(), &measure_diff);
				if (!pr) {
						dPrint(1,"Failed to get PositionEvent (timed)?");
						continue;
				} else {
						dPrint(6,"Got time difference: %s", measure_diff.toString().c_str());
				}
				}
				const Position::TPose2D *odo; ///< Odometry from the robot		
				odo = pd_timed.GetPose2D();  ///< pointer to odometry data
				if(odo == NULL){ fprintf(stderr,"Odo NULL?!?!?\n");	exit(1);}
				
				fprintf(f,"%.3f %.3f %.3f",odo->x,odo->y,odo->a);
		}else{
				fprintf(f,"%.3f %.3f %.3f",0.0,0.0,0.0);
		}
		for(int i=0;i<rd.GetDistanceElementCount();i++){
				r_meas = rd.GetDistance(i);
				fprintf(f," %.3f %.3f",r_meas->distance,r_meas->angle);
		}
		fprintf(f,"\n");
		fflush(f);
    
	}
		fclose(f);
  return 0;
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  // namespace to use
  using namespace MaCI::Position;
  
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string maciGroup;
 

  Position::CPositionClient pc(&g, 0); // Position client instance
  Ranging::CRangingClient rc(&g, 0); // Ranging client instance

  // App specific parameters
  std::string rangingSourceName;
  std::string positionSourceName;
	std::string InstanceName = "Dummy";

  // Execute signal handler here.
  InitSignalHandler();

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("Position_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nPositionRanging_Dummy - Example base \n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "n:u:p:g:G:f:P:R:I:hqvd");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
    case 'P': {
      // Take name & minor
      positionSourceName = optarg;
      fprintf(stderr,"Using %s as Position instance\n",positionSourceName.c_str());
      break;
    }
    case 'R': {
      rangingSourceName = optarg;
			fprintf(stderr,"Using %s as Ranging instance\n",rangingSourceName.c_str());
      break;
    }
      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;
				case 'd':
						disablePosition = true;
						dPrint(3,"Position interface Disabled!");
						break;

    case 'g':
		case 'G':
      maciGroup = optarg;
      dPrint(3,"Using MaCI Group: '%s'", optarg);
      break;
		case 'f':
			logfilename = optarg;
			dPrint(3,"Using '%s' as Logfile!", optarg);
			break;
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
				debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  int r;
  //GIMnet connection, leave these alone.
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
		 exit(1);
  } 
		dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
				gimnetAP.c_str(), 
				gimnetAPPort, 
				gimnetName.size()?gimnetName.c_str():"<anonymous>",
				gimi::getGimiErrorString(r).c_str());
		
		MaCI::EMaCIError e;
				
		/////////////////////////////////////////////////////////////////////////////
		/////////////////////////////////////////////////////////////////////////////
		/// GET ALL MACI SERVICES FROM GIMNET
		/////////////////////////////////////////////////////////////////////////////
		/////////////////////////////////////////////////////////////////////////////
		// MaCICtrl client
		MaCICtrl::CMaCICtrlClient ctrlclient(&g); ///< Open MaCICtrl client.
		
		MaCICtrl::TServiceEntryArray services; ///< the services
		
		if (ctrlclient.Open() == KMaCIOK) {
    // Get all MaCI services from local GIMnet.
				gimi::t_serviceList macictrlservicelist;
				if (ctrlclient.DiscoverMaCICtrlServices(macictrlservicelist,3000) == KMaCIOK) {	
				// Request information from all MaCI nodes on the Net.			
						if (ctrlclient.GetServiceArray(services, macictrlservicelist) != MaCI::KMaCIOK) {
								dPrint(1,"Failed to get service array - aborting");
								exit(1);
						}
				}else{
						dPrint(1,"Failed to receive MaCI services from the GIMnet - Aborting - We are Wery sorry!");
						exit(1);
				}
		}
		if(!disablePosition){
				pc.SetDefaultTarget(
						MaCICtrl::SMaCISL(maciGroup + ".MaCI_Position."+ positionSourceName),services);
		}
		rc.SetDefaultTarget(
						MaCICtrl::SMaCISL(maciGroup + ".MaCI_Ranging."+rangingSourceName),services);
		
		////// Execute Client Opens ///////
		// Open PositionClient
		if(!disablePosition){
				if (pc.Open() != MaCI::KMaCIOK) {
						dPrint(1,"WARNING: Failed to open Position source '%s'! (Still continuing, expect problems!)", 
						positionSourceName.c_str());
				}
		}

		// Open RangingClient
		if (rc.Open() != MaCI::KMaCIOK) {
		dPrint(1,"WARNING: Failed to open Ranging source '%s'! (Still continuing, expect problems!)",
						rangingSourceName.c_str());
		}
		
		////// Execute test section ///////
		r = ExecuteModule(pc, rc);
		////// Test section end //////
		
  

  return r;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


